#include "serial_kalman.h"

SerialWrapper::SerialWrapper(const std::string &port, unsigned int baudrate, boost::asio::serial_port_base::character_size bytesize,
                             boost::asio::serial_port_base::parity parity, boost::asio::serial_port_base::stop_bits stopbits,const uint16_t sample_interval)
    : port(port), baudrate(baudrate), bytesize(bytesize), parity(parity), stopbits(stopbits), measure_interval(sample_interval), serial(io) {}

SerialWrapper::SerialWrapper(const std::string &port, unsigned int baudrate, boost::asio::serial_port_base::character_size bytesize,
                             boost::asio::serial_port_base::parity parity, boost::asio::serial_port_base::stop_bits stopbits,const uint16_t sample_interval, const KalmanFilter& kalmanfilter)
    : port(port), baudrate(baudrate), bytesize(bytesize), parity(parity), stopbits(stopbits), measure_interval(sample_interval), serial(io), kalmanfilter_(kalmanfilter) {}

bool SerialWrapper::open_serial()
{
    try
    {
        serial.open(port);
        serial.set_option(boost::asio::serial_port_base::baud_rate(baudrate));
        serial.set_option(bytesize);
        serial.set_option(parity);
        serial.set_option(stopbits);
        std::cout << "Serial port configured: BaudRate = " << baudrate
                  << ", StopBits = " << stop_bits_to_string(stopbits)
                  << ", Parity = " << parity_to_string(parity) << std::endl;
        return true;
    }
    catch (boost::system::system_error &e)
    {
        std::cerr << "Error opening serial port: " << e.what() << std::endl;
        return false;
    }
}

void SerialWrapper::close()
{
    if (serial.is_open())
    {
        serial.close();
        std::cout << "Serial port closed" << std::endl;
    }
}

void SerialWrapper::write_data(const std::vector<uint8_t> &data)
{
    while (running)
    {
        try
        {
            boost::asio::write(serial, boost::asio::buffer(data));
            std::this_thread::sleep_for(std::chrono::milliseconds(measure_interval)); // simulate interval sending
        }
        catch (boost::system::system_error &e)
        {
            if (!running)
                break;
            std::cerr << "Error writing to port: " << e.what() << std::endl;
        }
    }
}

void SerialWrapper::read_data()
{
    while (running)
    {
        try
        {
            uint8_t buf[256];
            boost::system::error_code ec;
            size_t len = serial.read_some(boost::asio::buffer(buf), ec);
            if (ec)
            {
                if (!running)
                    break;
                std::cerr << "Error reading from port: " << ec.message() << std::endl;
            }
            else if (len > 0)
            {
                process_received_data(buf, len);
            }
        }
        catch (boost::system::system_error &e)
        {
            if (!running)
                break;
            std::cerr << "Error reading from port: " << e.what() << std::endl;
        }
    }
}

void SerialWrapper::process_received_data(uint8_t *data, size_t length)
{
    std::lock_guard<std::mutex> lock(data_mutex);
    data_queue.insert(data_queue.end(), data, data + length);

    while (data_queue.size() >= 7)
    {
        if (data_queue[0] == 0x01 && data_queue[1] == 0x03 && data_queue[2] == 0x02)
        {
            std::vector<uint8_t> valid_data(data_queue.begin(), data_queue.begin() + 7);

            int combined_value = (valid_data[3] << 8) | valid_data[4];
            // std::cout << "Measure Distance is: " << std::dec << combined_value << " cm" << std::endl;

            last_measurement = current_measurement;
            current_measurement = combined_value;

            last_time = current_time;
            current_time = std::chrono::steady_clock::now();

            data_queue.erase(data_queue.begin(), data_queue.begin() + 7);
        }
        else
        {
            data_queue.erase(data_queue.begin());
        }
    }
}

void SerialWrapper::common_speed_calculation(std::function<void(uint64_t, uint16_t, double, double, uint8_t)> specific_calculation)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(measure_interval));

    std::lock_guard<std::mutex> lock(data_mutex);
    if (last_measurement != 0 && current_measurement != 0)
    {
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_time).count();
        if (duration > 0)
        {
            // 记录时间戳
            auto now = std::chrono::system_clock::now();
            auto duration_timestamp = now.time_since_epoch();
            uint64_t timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(duration_timestamp).count();
            std::cout << "Timestamp: " << timestamp << std::endl;
            // 打印采样的距离差
            double distance_diff = current_measurement - last_measurement;
            std::cout << "Current Measurement: " << std::abs(current_measurement) << " cm" << std::endl;
            // 通过采样的距离差计算速度
            double time_diff = duration / 1000.0; // convert to seconds
            std::cout << "Time Difference: " << std::abs(time_diff) << " s" << std::endl;
            double real_speed = distance_diff / time_diff; // cm/s
            uint16_t dir = (real_speed < 0) ? 0 : 1;
            std::cout << "Speed of " << ((dir == 0) ? "approaching: " : "moving away: ") << std::abs(real_speed) << " cm/s" << std::endl;
            std::cout << "Dir of speed: " << dir << std::endl;

            specific_calculation(timestamp, current_measurement, time_diff, real_speed, dir);
        }
    }
}

void SerialWrapper::calculate_speed(SpeedCallback callback)
{
    while (running)
    {
        common_speed_calculation([this, &callback](uint64_t timestamp, uint16_t current_measurement, double time_diff, double real_speed, uint8_t dir) {

            save_to_csv(timestamp, current_measurement, time_diff, real_speed, dir);

            // Call the callback with the current values
            callback(timestamp, current_measurement, std::abs(real_speed), dir);
        });
    }
}

void SerialWrapper::kalman_estimate_speed(KalmanSpeedCallback kalman_callback)
{
    while (running)
    {
        common_speed_calculation([this, &kalman_callback](uint64_t timestamp, uint16_t current_measurement, double time_diff, double real_speed, uint8_t dir) {
            // 通过卡尔曼滤波来估计速度值
            Eigen::MatrixXd F(3, 3);
            F << 1, time_diff, time_diff * time_diff / 2,
                 0, 1, time_diff,
                 0, 0, 1;
            Eigen::VectorXd current_measurement_vector(1);
            current_measurement_vector(0) = current_measurement;
            KalmanFilterResult kalman_result = kalmanfilter_.filter(current_measurement_vector, F);
            double kalman_dis = kalman_result.state[0];
            double kalman_vel = kalman_result.state[1];
            std::cout << "Kalman Distance: " << kalman_dis << " cm" << std::endl;
            std::cout << "Kalman Velocity: " << kalman_vel << " cm/s" << std::endl;

            save_to_csv(timestamp, time_diff, current_measurement, real_speed, dir, kalman_dis, kalman_vel);

            // Call the callback with the current values
            kalman_callback(timestamp, current_measurement, std::abs(real_speed), dir, kalman_dis, kalman_vel);
        });
    }
}

void SerialWrapper::save_to_csv(int64_t timestamp, double time_diff, int measurement, double speed, int dir)
{
    std::ifstream infile(filename);
    if (infile.good())
    {
        std::ofstream file(filename, std::ios::app);
        if (file.is_open())
        {
            file << timestamp << "," << time_diff << "," << measurement << "," << speed << "," << dir << "\n";
        }
        else
        {
            std::cerr << "Error opening file to save data" << std::endl;
        }
    }
}

void SerialWrapper::save_to_csv(int64_t timestamp, double time_diff, int measurement, double speed, int dir, double kalman_dis, double kalman_vel)
{
    std::ifstream infile(filename);
    if (infile.good())
    {
        std::ofstream file(filename, std::ios::app);
        if (file.is_open())
        {
            file << timestamp << "," << time_diff << "," << measurement << "," << speed << "," << dir << "," << kalman_dis << "," << kalman_vel << "\n";
        }
        else
        {
            std::cerr << "Error opening file to save data" << std::endl;
        }
    }
}

void SerialWrapper::ensure_file_exists(const char* file_name, bool kalman_file = false)
{
    filename = file_name;
    std::ifstream infile(file_name);

    if (!infile.good())
    {
        std::ofstream file(file_name, std::ios::app);
        if (file.is_open())
        {
            if (kalman_file)
            {
                file << "Timestamp,Time_Diff(s),real_Distance(cm),real_Speed(cm/s),real_dir(0down1up),kal_Distance(cm),kal_Speed(cm/s)\n";
            }else{
                file << "Timestamp,Time_Diff(s),real_Distance(cm),real_Speed(cm/s),real_dir(0down1up)\n";
            }
        }
    }
}

std::string SerialWrapper::stop_bits_to_string(boost::asio::serial_port_base::stop_bits stopbits)
{
    switch (stopbits.value())
    {
    case boost::asio::serial_port_base::stop_bits::one:
        return "1";
    case boost::asio::serial_port_base::stop_bits::onepointfive:
        return "1.5";
    case boost::asio::serial_port_base::stop_bits::two:
        return "2";
    default:
        return "unknown";
    }
}

std::string SerialWrapper::parity_to_string(boost::asio::serial_port_base::parity parity)
{
    switch (parity.value())
    {
    case boost::asio::serial_port_base::parity::none:
        return "none";
    case boost::asio::serial_port_base::parity::odd:
        return "odd";
    case boost::asio::serial_port_base::parity::even:
        return "even";
    default:
        return "unknown";
    }
}
